
#include "piddemo.h"

#include "ti_msp_dl_config.h"
#include "app/text.h"
#include "hardware/encoder.h"
#include "hardware/motor.h"
#include "mid/pid.h"
#include "mid/printf.h"

#include <string.h>

PID gPID;
char gTuning = 0;
char gPText[8] = { 0 };
char gIText[8] = { 0 };
char gDText[8] = { 0 };
char gSpeedText[8] = { 0 };
char gAngleText[8] = { 0 };
char gTargetText[16] = { 0 };
int gSelected = 0;

void MotorSpeedControl(PID* pid)
{
    int pwm = 0;

    pwm = CalculatePID(pid, pid->target, GetEncoderValue());
    if (pwm > 0)
    {
        SetMotor(0, pwm);
    }
    else if (pwm < 0)
    {
        SetMotor(-pwm, 0);
    }
    // printf("speed pwd=%d\n", pwm);
}

void MotorDistanceControl(PID* pid)
{
    int pulses = (float)pid->target / DEGREES_PER_PULSE;
    int pwm = CalculatePID(pid, pulses, GetRealtimeEncoderValue());

    if (pwm > 0)
    {
        SetMotor(0, pwm);
    }
    else
    {
        SetMotor(-pwm, 0);
    }
    // printf("angle pwd=%d\n", pwm);
}

void UpdatePIDText(PID* pid, eUpdatePID flag)
{
    if (flag & ePID_P)
    {
        memset(gPText, 0, sizeof(gPText));
        sprintf(gPText, "%0.2f", pid->kP);
    }
    if (flag & ePID_I)
    {
        memset(gIText, 0, sizeof(gIText));
        sprintf(gIText, "%0.2f", pid->kI);
    }
    if (flag & ePID_D)
    {
        memset(gDText, 0, sizeof(gDText));    
        sprintf(gDText, "%0.2f", pid->kD);
    }
    if (flag & ePID_TARGET)
    {
        memset(gTargetText, 0, sizeof(gTargetText));    
        sprintf(gTargetText, "%s%3d", LABEL_TARGET, pid->target);
    }
}

void UpdatePIDTargetValue(PID* pid, int delta)
{
    switch (gSelected)
    {
    case ePID_BTN_P:
        pid->kP += delta * PID_STEP;
        if (pid->kP < 0.f)
            pid->kP = 0.f;
        UpdatePIDText(pid, ePID_P);
        break;
    case ePID_BTN_I:
        pid->kI += delta * PID_STEP;
        if (pid->kI < 0.f)
            pid->kI = 0.f;
        UpdatePIDText(pid, ePID_I);
        break;
    case ePID_BTN_D:
        pid->kD += delta * PID_STEP;
        if (pid->kD < 0.f)
            pid->kD = 0.f;
        UpdatePIDText(pid, ePID_D);
        break;
    case ePID_BTN_TARGET:
        pid->target += delta * TARGET_STEP;
        if (pid->target > pid->maxTarget)
            pid->target = pid->maxTarget;
        else if (pid->target < -pid->maxTarget)
            pid->target = -pid->maxTarget;
        UpdatePIDTargetText(pid);
        break;
    }
}

void UpdatePIDTargetText(PID* pid)
{
    memset(gTargetText, 0, sizeof(gTargetText));
    sprintf(gTargetText, "%s%3d", LABEL_TARGET, pid->target);
}